#ifndef __GAMETOOLES__
#define __GAMETOOLES__

#ifdef __cplusplus
extern "C"
{
#endif
#include "math.h"

#ifdef __cplusplus
}
#endif

#include "USB.hpp"

#define _DEG 0.0174533f

typedef struct
{
    uint8_t reprotid;
    uint8_t dead_Rx_Low;
    uint8_t dead_Rx_High;
    uint8_t dead_Ry_Low;
    uint8_t dead_Ry_High;
    uint8_t dead_Brake_Low;
    uint8_t dead_Brake_High;
    uint8_t dead_Throttle_Low;
    uint8_t dead_Throttle_High;
    uint8_t wave_filtering;
    union
    {
        struct
        {
            uint8_t ENABLE_Rx : 1;
            uint8_t ENABLE_Ry : 1;
            uint8_t ENABLE_Brake : 1;
            uint8_t ENABLE_Throttle : 1;
            uint8_t MODE_1 : 1;
            uint8_t MODE_2 : 1;
            uint8_t MODE_3 : 1;
            uint8_t MODE_4 : 1;
        };
        struct
        {
            uint8_t ENABLE_Flags : 4;
            uint8_t MODE_Flags : 4;
        };
        uint8_t Flags;
    };
    uint8_t temp;
    uint16_t temp1;
    uint16_t temp2;
    uint16_t temp3;
    uint16_t temp4;
} out_reprot;

typedef struct
{
    uint8_t reportid;
    uint8_t Wheel_Flag;
    uint8_t Force;
    uint8_t Friction;
    uint8_t Gain;
    uint8_t Antigravity;
    int16_t Wheel_Engle;
    uint16_t Wheel_EncoderCPR;
    uint16_t Wheel_Degrees;
    uint8_t Wheel_stop;
    uint8_t Wheel_center;
    uint16_t temp1;
    uint16_t temp2;
    uint16_t temp3;
    uint16_t Wheel_offset;
} Wheel_config;

typedef struct
{
    float LastP; //上次估算协方差
    float NowP;  //当前估算协方差
    float out;   //滤波输出
    float Kg;    //卡尔曼增益
    float Q;     //过程噪声协方差
    float R;     //观测噪声协方差
} Kalman_Filter_Par;

void Axis_X4(reprot_type *reprot);
void Amplitude_Filter(reprot_type *reprot, uint16_t wava_filter);
void Filter(reprot_type *reprot, uint16_t wava_filter);
void Low_Pass_Filter(reprot_type *reprot, uint16_t wava_filter);
float KalmanFilter(Kalman_Filter_Par *kfp, float input);

void Axis_Config(reprot_type *reprot, Dead_Axis *Dead);
void Axis_Init_Config(Dead_Axis *dead, out_reprot *config);
#endif